{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# Cal3_S2Stereo"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/geometry/doc/Cal3_S2Stereo.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "A `Cal3_S2Stereo` object describes the calibration model for a stereo camera rig, a setup for depth estimation where two cameras are mounted side by side at a fixed horizontal distance known as the baseline. This model assumes both cameras have the same intrinsic parameters.\n",
        "\n",
        "This class inherits from the standard 5-parameter [`Cal3_S2`](./Cal3_S2.ipynb) model and adds a sixth parameter, the baseline $b$. \n",
        "\n",
        "The calibration matrix $K$ is identical to that of [`Cal3_S2`](./Cal3_S2.ipynb) and represents the parameters of a single camera from the stereo camera rig:\n",
        "\n",
        "$$\n",
        "K = \\begin{bmatrix} f_x & s & u_0 \\\\ 0 & f_y & v_0 \\\\ 0 & 0 & 1 \\end{bmatrix}\n",
        "$$\n",
        "\n",
        "However, note that `Cal3_S2Stereo` itself does not perform the complete calibration of a stereo camera; it merely stores all the parameters necessary for the calibration. For instance, the baseline parameter $b$ is primarily used by other classes like [`StereoCamera`](./StereoCamera.ipynb) to compute the disparity between the left and right images during 3D projection.\n",
        "\n",
        "To see how `Cal3_S2Stereo` is used in practical applications, please refer to [StereoVOExample](../../../python/gtsam/examples/StereoVOExample.ipynb) and [StereoVOExample_large](../../../python/gtsam/examples/StereoVOExample_large.ipynb)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Initialization\n",
        "\n",
        "A `Cal3_S2Stereo` object can be initialized in several ways:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Default constructor (fx=1, fy=1, s=0, u0=0, v0=0, b=1):\n",
            "K: 1 0 0\n",
            "0 1 0\n",
            "0 0 1\n",
            "Baseline: 1\n",
            "\n",
            "fx: 1.0, fy: 1.0, s: 0.0, u0: 0.0, v0: 0.0, 1.0\n",
            "\n",
            "From parameters (fx, fy, s, u0, v0, b):\n",
            "K: 1500    0  320\n",
            "   0 1600  240\n",
            "   0    0    1\n",
            "Baseline: 0.5\n",
            "\n",
            "fx: 1500.0, fy: 1600.0, s: 0.0, u0: 320.0, v0: 240.0, 0.5\n",
            "\n",
            "From a 6-vector [fx, fy, s, u0, v0, b]:\n",
            "K: 1500    0  320\n",
            "   0 1600  240\n",
            "   0    0    1\n",
            "Baseline: 0.5\n",
            "\n",
            "fx: 1500.0, fy: 1600.0, s: 0.0, u0: 320.0, v0: 240.0, 0.5\n",
            "\n",
            "From fov, image dimensions, and baseline:\n",
            "K: 251.73      0    300\n",
            "     0 251.73    250\n",
            "     0      0      1\n",
            "Baseline: 0.5\n",
            "\n",
            "fx: 1500.0, fy: 1600.0, s: 0.0, u0: 320.0, v0: 240.0, 0.5\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Default constructor\n",
        "cal_default = gtsam.Cal3_S2Stereo()\n",
        "print(\"Default constructor (fx=1, fy=1, s=0, u0=0, v0=0, b=1):\")\n",
        "print(cal_default)\n",
        "print(f\"fx: {cal_default.fx()}, fy: {cal_default.fy()}, s: {cal_default.skew()}, u0: {cal_default.px()}, v0: {cal_default.py()}, {cal_default.baseline()}\\n\")\n",
        "\n",
        "# From individual parameters: fx, fy, s, u0, v0, b\n",
        "fx, fy, s, u0, v0, b = 1500.0, 1600.0, 0, 320.0, 240.0, 0.5\n",
        "cal_params = gtsam.Cal3_S2Stereo(fx, fy, s, u0, v0, b)\n",
        "print(\"From parameters (fx, fy, s, u0, v0, b):\")\n",
        "print(cal_params)\n",
        "print(f\"fx: {cal_params.fx()}, fy: {cal_params.fy()}, s: {cal_params.skew()}, u0: {cal_params.px()}, v0: {cal_params.py()}, {cal_params.baseline()}\\n\")\n",
        "\n",
        "# From a 6-vector [fx, fy, s, u0, v0, b]\n",
        "param_vector = np.array([1500.0, 1600.0, 0, 320.0, 240.0, 0.5])\n",
        "cal_vector = gtsam.Cal3_S2Stereo(param_vector)\n",
        "print(\"From a 6-vector [fx, fy, s, u0, v0, b]:\")\n",
        "print(cal_vector)\n",
        "print(f\"fx: {cal_vector.fx()}, fy: {cal_vector.fy()}, s: {cal_vector.skew()}, u0: {cal_vector.px()}, v0: {cal_vector.py()}, {cal_vector.baseline()}\\n\")\n",
        "\n",
        "# From fov (in degrees), image dimensions, and stereo baseline\n",
        "fov, w, h, b = 100, 600, 500, 0.5\n",
        "cal_fov = gtsam.Cal3_S2Stereo(fov, w, h, b)\n",
        "print(\"From fov, image dimensions, and baseline:\")\n",
        "print(cal_fov)\n",
        "print(f\"fx: {cal_vector.fx()}, fy: {cal_vector.fy()}, s: {cal_vector.skew()}, u0: {cal_vector.px()}, v0: {cal_vector.py()}, {cal_vector.baseline()}\\n\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Properties\n",
        "\n",
        "Since `Cal3_S2Stereo` inherits from [`Cal3_S2`](./Cal3_S2.ipynb), it has access to all of its parent's property member functions, in addition to its own specific member functions, as follows:\n",
        "- `baseline()`: Returns the horizontal baseline $b$."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Calibration object properties:\n",
            "fx: 1500.0\n",
            "fy: 1600.0\n",
            "skew: 0.0\n",
            "u0: 320.0\n",
            "v0: 240.0\n",
            "Baseline: 0.5\n",
            "Principal Point: [320. 240.]\n",
            "Vector [fx, fy, s, u0, v0, b]: [1.5e+03 1.6e+03 0.0e+00 3.2e+02 2.4e+02 5.0e-01]\n",
            "Aspect ratio: 0.9375\n",
            "K matrix:\n",
            "[[1.5e+03 0.0e+00 3.2e+02]\n",
            " [0.0e+00 1.6e+03 2.4e+02]\n",
            " [0.0e+00 0.0e+00 1.0e+00]]\n"
          ]
        }
      ],
      "source": [
        "fx, fy, s, u0, v0, b = 1500.0, 1600.0, 0, 320.0, 240.0, 0.5\n",
        "cal = gtsam.Cal3_S2Stereo(fx, fy, s, u0, v0, b)\n",
        "\n",
        "print(\"Calibration object properties:\")\n",
        "print(f\"fx: {cal.fx()}\")\n",
        "print(f\"fy: {cal.fy()}\")\n",
        "print(f\"skew: {cal.skew()}\")\n",
        "print(f\"u0: {cal.px()}\")\n",
        "print(f\"v0: {cal.py()}\")\n",
        "print(f\"Baseline: {cal.baseline()}\")\n",
        "print(f\"Principal Point: {cal.principalPoint()}\")\n",
        "print(f\"Vector [fx, fy, s, u0, v0, b]: {cal.vector()}\")\n",
        "print(f\"Aspect ratio: {cal.aspectRatio()}\")\n",
        "print(f\"K matrix:\\n{cal.K()}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Basic Operations\n",
        "\n",
        "### `calibrate()`\n",
        "\n",
        "The `calibrate(p)` function converts a 2D point `p` from pixel coordinates $(u, v)$ to intrinsic coordinates $(x, y)$. Please review the user guide for [`Cal3_S2`](./Cal3_S2.ipynb) for a more detailed explanation of this function."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "image[0, 0] -> (u, v)=(0.5px, 0.5px) -> (x, y)=(-0.213, -0.14969)\n",
            "image[768, 1024] -> (u, v)=(1024.5px, 768.5px) -> (x, y)=(0.46967, 0.33031)\n",
            "image[1536, 2048] -> (u, v)=(2048.5px, 1536.5px) -> (x, y)=(1.15233, 0.81031)\n"
          ]
        }
      ],
      "source": [
        "def calibration_demo(cal: gtsam.Cal3_S2Stereo, row: int, col: int):\n",
        "  u, v = col + 0.5, row + 0.5   # coordinates of pixel center\n",
        "  x, y = cal.calibrate([u, v])\n",
        "  print(f\"image[{row}, {col}] -> (u, v)=({round(u, 2)}px, {round(v, 2)}px) -> (x, y)=({round(x, 5)}, {round(y, 5)})\")\n",
        "  return x, y\n",
        "\n",
        "fx, fy, s, u0, v0, b = 1500.0, 1600.0, 0, 320.0, 240.0, 0.5\n",
        "cal = gtsam.Cal3_S2Stereo(fx, fy, s, u0, v0, b)\n",
        "\n",
        "_ = calibration_demo(cal, 0, 0)\n",
        "_ = calibration_demo(cal, 768, 1024)\n",
        "_ = calibration_demo(cal, 1536, 2048)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "`calibrate(p)` can also optionally compute Jacobians."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Jacobian Dcal_calibrate:\n",
            "[[-0.00019911  0.         -0.00032667 -0.00066667  0.          0.        ]\n",
            " [ 0.         -0.00030625  0.          0.         -0.000625    0.        ]]\n",
            "\n",
            "Jacobian Dp_calibrate:\n",
            "[[ 0.00066667 -0.        ]\n",
            " [ 0.          0.000625  ]]\n"
          ]
        }
      ],
      "source": [
        "Dcal_calibrate = np.zeros((2, 6), order='F') # Note shape is 2x6\n",
        "Dp_calibrate = np.zeros((2, 2), order='F')\n",
        "p_pixels = [768, 1024]\n",
        "_ = cal.calibrate(p_pixels, Dcal_calibrate, Dp_calibrate)\n",
        "print(f\"\\nJacobian Dcal_calibrate:\\n{Dcal_calibrate}\")\n",
        "print(f\"\\nJacobian Dp_calibrate:\\n{Dp_calibrate}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### `uncalibrate()`\n",
        "\n",
        "The `uncalibrate(p)` method converts a 2D point `p` from intrinsic coordinates $(x,y)$ back to pixel coordinates $(u, v)$. Please review the user guide for [`Cal3_S2`](./Cal3_S2.ipynb) for a more detailed explanation of this function."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "image[0, 0] -> (u, v)=(0.5px, 0.5px) -> (x, y)=(-0.213, -0.14969)\n",
            "(x, y)=(-0.213, -0.14969) -> (u, v)=(0.5px, 0.5px) -> image[0.0, 0.0]\n",
            "image[1024, 768] -> (u, v)=(768.5px, 1024.5px) -> (x, y)=(0.299, 0.49031)\n",
            "(x, y)=(0.299, 0.49031) -> (u, v)=(768.5px, 1024.5px) -> image[1024.0, 768.0]\n"
          ]
        }
      ],
      "source": [
        "def uncalibration_demo(cal: gtsam.Cal3_S2Stereo, x: float, y: float):\n",
        "  u, v = cal.uncalibrate([x, y])\n",
        "  print(f\"(x, y)=({round(x, 5)}, {round(y, 5)}) -> (u, v)=({round(u, 2)}px, {round(v, 2)}px) -> image[{np.floor(v)}, {np.floor(u)}]\")\n",
        "  return u, v\n",
        "\n",
        "x, y = calibration_demo(cal, 0, 0)\n",
        "_ = uncalibration_demo(cal, x, y)\n",
        "x, y = calibration_demo(cal, 1024, 768)\n",
        "_ = uncalibration_demo(cal, x, y)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "`uncalibrate(p)` can optionally compute Jacobians as well."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Jacobian Dcal_uncalibrate:\n",
            "[[0. 0. 0. 1. 0. 0.]\n",
            " [0. 0. 0. 0. 1. 0.]]\n",
            "\n",
            "Jacobian Dp_uncalibrate:\n",
            "[[1500.    0.]\n",
            " [   0. 1600.]]\n"
          ]
        }
      ],
      "source": [
        "# This method can also optionally compute Jacobians.\n",
        "Dcal_uncalibrate = np.zeros((2, 6), order='F')\n",
        "Dp_uncalibrate = np.zeros((2, 2), order='F')\n",
        "p_intrinsic = [0, 0]\n",
        "_ = cal.uncalibrate(p_intrinsic, Dcal_uncalibrate, Dp_uncalibrate)\n",
        "print(f\"\\nJacobian Dcal_uncalibrate:\\n{Dcal_uncalibrate}\")\n",
        "print(f\"\\nJacobian Dp_uncalibrate:\\n{Dp_uncalibrate}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Manifold Operations\n",
        "\n",
        "Like other calibration classes in GTSAM, `Cal3_S2Stereo` is treated as a manifold type. This abstraction allows it to integrate seamlessly into GTSAM's optimization framework.\n",
        "\n",
        "- `retract(d)` maps a small update vector `d` in the tangent space to a new calibration object on the manifold.\n",
        "- `localCoordinates(T2)` computes the tangent-space vector that moves from the current calibration to another calibration `T2`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Original cal_model:\n",
            "K: 1500    0  320\n",
            "   0 1600  240\n",
            "   0    0    1\n",
            "Baseline: 0.5\n",
            "\n",
            "\n",
            "Delta vector: [ 1.e+01  2.e+01  5.e-02  1.e+00 -1.e+00  1.e-02]\n",
            "\n",
            "Retracted cal_retracted:\n",
            "K: 1510 0.05  321\n",
            "   0 1620  239\n",
            "   0    0    1\n",
            "Baseline: 0.51\n",
            "\n",
            "\n",
            "Local coordinates from cal_model to cal_retracted:\n",
            "[ 1.e+01  2.e+01  5.e-02  1.e+00 -1.e+00  1.e-02]\n"
          ]
        }
      ],
      "source": [
        "print(\"Original cal_model:\")\n",
        "print(cal)\n",
        "\n",
        "# Retract: Apply a delta to the calibration parameters\n",
        "delta_vec = np.array([10.0, 20.0, 0.05, 1.0, -1.0, 0.01])\n",
        "cal_retracted = cal.retract(delta_vec)\n",
        "print(f\"\\nDelta vector: {delta_vec}\")\n",
        "print(\"\\nRetracted cal_retracted:\")\n",
        "print(cal_retracted)\n",
        "\n",
        "# Local coordinates: Find the delta between two calibrations\n",
        "local_coords = cal.localCoordinates(cal_retracted)\n",
        "print(\"\\nLocal coordinates from cal_model to cal_retracted:\")\n",
        "print(local_coords)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Relationship with `StereoCamera`\n",
        "\n",
        "The `Cal3_S2Stereo` object should be used with the [`StereoCamera`](./StereoCamera.ipynb) class. A [`StereoCamera`](./StereoCamera.ipynb) combines a [`Pose3`](./Pose3.ipynb) (the pose of the left camera) and a `Cal3_S2Stereo` (holding the calibration parameters) to project 3D points into the stereo image plane, producing a [`StereoPoint2`](./StereoPoint2.ipynb) which contains the left and right image coordinates ($u_L, u_R, v$). Below is a basic example; please look at the user guide of [`StereoCamera`](./StereoCamera.ipynb) for a more thorough explanation."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 10,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "3D Point in world frame: [5. 3. 2.]\n",
            "StereoCamera pose:\n",
            "R: [\n",
            "\t6.12323e-17, 6.12323e-17, 1;\n",
            "\t-1, 3.7494e-33, 6.12323e-17;\n",
            "\t-0, -1, 6.12323e-17\n",
            "]\n",
            "t: 0 0 5\n",
            "\n",
            "Stereo calibration:\n",
            "K: 1000    0  640\n",
            "   0 1000  360\n",
            "   0    0    1\n",
            "Baseline: 0.5\n",
            "\n",
            "\n",
            "Projected StereoPoint2 (u_L, u_R, v): (40, -60, 960)\n",
            "\n"
          ]
        }
      ],
      "source": [
        "fx, fy, s, u0, v0, b = 1000, 1000, 0, 640, 360, 0.5\n",
        "stereo_calibration = gtsam.Cal3_S2Stereo(fx, fy, s, u0, v0, b)\n",
        "\n",
        "# Define the pose of the stereo camera rig in the world frame (left camera's pose)\n",
        "camera_pose = gtsam.Pose3(gtsam.Rot3.Ypr(-np.pi/2, 0, -np.pi/2), [0, 0, 5.0])\n",
        "\n",
        "# Create a StereoCamera object\n",
        "stereo_camera = gtsam.StereoCamera(camera_pose, stereo_calibration)\n",
        "\n",
        "# Define a 3D point in the world frame\n",
        "p_world = gtsam.Point3(5, 3, 2)\n",
        "\n",
        "# Project the 3D point into the stereo camera\n",
        "p_stereo = stereo_camera.project(p_world)\n",
        "\n",
        "print(f\"3D Point in world frame: {p_world}\")\n",
        "print(f\"StereoCamera pose:\\n{stereo_camera.pose()}\")\n",
        "print(f\"Stereo calibration:\\n{stereo_camera.calibration()}\")\n",
        "print(f\"\\nProjected StereoPoint2 (u_L, u_R, v): {p_stereo}\")\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Additional Resources\n",
        "\n",
        "The following resources provide more detailed explanations of camera calibration and stereo vision.\n",
        "\n",
        "### Article(s)\n",
        "- [\"5.3. Cameras for Robot Vision\"](https://www.roboticsbook.org/S53_diffdrive_sensing.html) by Dr. Frank Dellaert and Dr. Seth Hutchinson\n",
        "\n",
        "### Video(s)\n",
        "- [\"Simple Stereo | Camera Calibration\"](https://youtu.be/hUVyDabn1Mg?si=3zIrPPML-H7Zu5_i) by Dr. Shree Nayar from Columbia University\n",
        "\n",
        "## Source\n",
        "- [Cal3_S2Stereo.h](https://github.com/borglab/gtsam/blob/develop/gtsam/geometry/Cal3_S2Stereo.h)\n",
        "- [Cal3_S2Stereo.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/geometry/Cal3_S2Stereo.cpp)"
      ]
    }
  ],
  "metadata": {
    "colab": {
      "provenance": []
    },
    "kernelspec": {
      "display_name": "base",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.10"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}